/**
 * Copyright (C) 2010  ARToolkitPlus Authors
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors:
 *  Daniel Wagner
 *  Pavel Rojtberg
 *  Krzysztof Skrzypski
 */

#define LOGE(...) ((void)__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__))
#define LOGI(...) ((void)__android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__))
#define LOG_TAG "ARToolkitPlusMain.cpp"
#include <../marker_info.h>
#include <MainSettings.h>
#include <android/log.h>
#include <cstdio>
#include <string.h>

//simple and BCH markers woks with:
TrackerSingleMarker tracker(width1, height1, 8, 6, 6, 6, 1);//valid marker id sizes: 6x6, 12x12 or 18x18.
//template marker detection works with those parameters, valid marker size is 16x16
TrackerSingleMarker trackerTemplate(width1, height1, 8, 16, 16, 64, 1); //va
//muti marker tracking mode:
TrackerMultiMarker trackerMultiTemplate(width1, height1, 8, 16, 16, 64, 8); 
TrackerMultiMarker trackerMulti(width1, height1, 8, 6, 6, 6, 20); 

JNIEXPORT void JNICALL Java_com_artollkitplus_benchmark_MarkerInfo_artoolkitPlus_1init(
		JNIEnv *env, jobject object, jint imageWidth, jint imageHeight,
		jint screenWidth, jint screenHeight,jobject fd_sys, jlong off, jlong len, jlong currentARSettings, jstring sdCardDir) {
	long currSettings = (long)currentARSettings;
	
	if((long)(currSettings&BIT_MODE_MARKER_TYPE_TEMPLATE)>0){
		marker_mode = TEMPLATE;
	}else{
		marker_mode = NOT_TEMPLATE;
	}
	
	////////////////////////////////////////////////////////////////////////////
	// switch this between true and false to test
	// simple-id versus BCH-id markers
	bool useBCH = false;
	if((long)(currSettings&BIT_MODE_MARKER_TYPE_BCH)>0){
		useBCH = true;
	}
	const int width = imageHeight, height = imageHeight, bpp = 1;//luminance
	size_t numPixels = width * height * bpp;
	////////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////
	if((long)(currSettings&BIT_MODE_TRACKER_BASE_TYPE_SINGLE)>0){
		tracker_mode = TRACKER_SINGLE;
	}else{
		tracker_mode = TRACKER_MULTI;
	}
	////////////////////////////////////////////////////////////////////////////
	if((long)(currSettings&BIT_MODE_MARKER_TYPE_BCH)>0){
		tracker.setMarkerMode(ARToolKitPlus::MARKER_ID_BCH); 
		trackerTemplate.setMarkerMode(ARToolKitPlus::MARKER_ID_BCH); 
		trackerMulti.setMarkerMode(ARToolKitPlus::MARKER_ID_BCH); 
		trackerMultiTemplate.setMarkerMode(ARToolKitPlus::MARKER_ID_BCH); 	
	}else if((long)(currSettings&BIT_MODE_MARKER_TYPE_SIMPLE)>0){
		tracker.setMarkerMode(ARToolKitPlus::MARKER_ID_SIMPLE); 
		trackerTemplate.setMarkerMode(ARToolKitPlus::MARKER_ID_SIMPLE); 
		trackerMulti.setMarkerMode(ARToolKitPlus::MARKER_ID_SIMPLE); 
		trackerMultiTemplate.setMarkerMode(ARToolKitPlus::MARKER_ID_SIMPLE);
	}else{
		tracker.setMarkerMode(ARToolKitPlus::MARKER_TEMPLATE); 
		trackerTemplate.setMarkerMode(ARToolKitPlus::MARKER_TEMPLATE); 
		trackerMulti.setMarkerMode(ARToolKitPlus::MARKER_TEMPLATE); 
		trackerMultiTemplate.setMarkerMode(ARToolKitPlus::MARKER_TEMPLATE);	
	}
	/////////////////////////////////////////////////////////////////////////
	if((long)(currSettings&BIT_MODE_POSE_ESTIMATOR_ORIGINAL)>0){
		tracker.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL); 
		trackerTemplate.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL); 
		trackerMulti.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL); 
		trackerMultiTemplate.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL);	
	}else if((long)(currSettings&BIT_MODE_POSE_ESTIMATOR_ORIGINAL_CONT)>0){
		tracker.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL_CONT); 
		trackerTemplate.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL_CONT); 
		trackerMulti.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL_CONT); 
		trackerMultiTemplate.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL_CONT);
	}else{
		tracker.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP); 
		trackerTemplate.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP); 
		trackerMulti.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP); 
		trackerMultiTemplate.setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);
	}
	////////////////////////////////////////////////////////////////////////////
	const char *nativeString = env->GetStringUTFChars(sdCardDir, NULL);
	const char *cameraCal = (char *)"/ARToolKitPlusBenchmark/camera_calibration.cal";
	char cameraCalPath[400]; 
	strcpy(cameraCalPath,nativeString);
	strcat(cameraCalPath,cameraCal);
	const char *markerData = (char *)"/ARToolKitPlusBenchmark/marker.dat";
	char markerDataPath[400];
	strcpy(markerDataPath,nativeString);
	strcat(markerDataPath,markerData);
	const char *markerDataHiro = (char *)"/ARToolKitPlusBenchmark/patt.hiro";
	char markerDataPathHiro[400];
	strcpy(markerDataPathHiro,nativeString);
	strcat(markerDataPathHiro,markerDataHiro);
	const char *markerBoard = (char *)"/ARToolKitPlusBenchmark/markerboard_480_499_cfg.dat";
	char markerBoardPath[400];
	strcpy(markerBoardPath,nativeString);
	strcat(markerBoardPath,markerBoard);

	if(tracker_mode == TRACKER_MULTI){
		/////////////////////////////TEMPLATE MULTI//////////////////////////////////////////
		trackerMultiTemplate.changeCameraSize(imageWidth,imageHeight);
		
		if (!trackerMultiTemplate.init(cameraCalPath,markerDataPath, 1.0f, 1000.0f,nativeString))
		{
			LOGE("ERROR: Camera trackerMultiTemplate.init() failed\n");
		}
		trackerMultiTemplate.setBorderWidth(useBCH ? 0.125 : 0.25);
		trackerMultiTemplate.setThreshold(150);
		trackerMultiTemplate.activateAutoThreshold(true);
		trackerMultiTemplate.setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
		trackerMultiTemplate.setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);
		if((long)(currSettings&BIT_MODE_HULL_OFF)>0){
			trackerMultiTemplate.setHullMode(ARToolKitPlus::HULL_OFF);
			trackerMulti.setHullMode(ARToolKitPlus::HULL_OFF);
		}else if((long)(currSettings&BIT_MODE_HULL_FOUR)>0){
			trackerMultiTemplate.setHullMode(ARToolKitPlus::HULL_FOUR);
			trackerMulti.setHullMode(ARToolKitPlus::HULL_FOUR);
		}else{
			trackerMultiTemplate.setHullMode(ARToolKitPlus::HULL_FULL);
			trackerMulti.setHullMode(ARToolKitPlus::HULL_FULL);
		}
		/////////////////////////////TEMPLATE MULTI//////////////////////////////////////////
		/////////////////////////////ID MULTI////////////////////////////////////////////////
		trackerMulti.changeCameraSize(imageWidth,imageHeight);
		if (!trackerMulti.init(cameraCalPath,markerBoardPath, 1.0f, 1000.0f,nativeString))
		{
			LOGE("ERROR: Camera trackerMulti.init() failed\n");
		}
		trackerMulti.setBorderWidth(useBCH ? 0.125 : 0.25);
		trackerMulti.setThreshold(150);
		trackerMulti.activateAutoThreshold(true);
		trackerMulti.setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
		trackerMulti.setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);
		/////////////////////////////ID MULTI////////////////////////////////////////////////
	}else if(tracker_mode == TRACKER_SINGLE){
		// create a tracker that does:
		//  - 6x6 sized marker images (required for binary markers)
		//  - samples at a maximum of 6x6
		//  - works with luminance (gray) images
		//  - can load a maximum of 0 non-binary pattern
		//  - can detect a maximum of 8 patterns in one imagege
		tracker.changeCameraSize(imageWidth,imageHeight);
		trackerTemplate.changeCameraSize(imageWidth,imageHeight);
		// load a camera file.
		if (!tracker.init(cameraCalPath, 1.0f, 1000.0f)) // load MATLAB file
		{
			LOGE("ERROR: Camera tracker.init() failed\n");
		}
		if (!trackerTemplate.init(cameraCalPath, 1.0f, 1000.0f)) // load MATLAB file
		{
			LOGE("ERROR: Camera init() trackerTemplate.failed\n");
		}
		
		if(initialized<1){
			if( (patternID=trackerTemplate.addPattern(markerDataPathHiro)) < 0 ) {
				LOGE("trackerTemplate: pattern load error");
			}
		}

		//tracker.getCamera()->printSettings();

		// define size of the marker in OpenGL units
		tracker.setPatternWidth(2.0);
		trackerTemplate.setPatternWidth(2.0);
		// the marker in the BCH test image has a thin border...
		tracker.setBorderWidth(useBCH ? 0.125 : 0.25);
		trackerTemplate.setBorderWidth(useBCH ? 0.125 : 0.25);
		// set a threshold. alternatively we could also activate automatic thresholding
		tracker.setThreshold(150);
		tracker.activateAutoThreshold(true);
		trackerTemplate.setThreshold(150);
		trackerTemplate.activateAutoThreshold(true);
		// let's use lookup-table undistortion for high-speed
		// note: LUT only works with images up to 1024x1024
		tracker.setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
		trackerTemplate.setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
		// switch to simple ID based markers
		// use the tool in tools/IdPatGen to generate markers
		///////////////////////////////////////////////////////////////////////
		///////////////////////////////////////////////////////////////////////
		//tracker.setHullMode(ARToolKitPlus::HULL_OFF);
		//HULL_OFF, HULL_FOUR, HULL_FULL
		//UNDIST_NONE, UNDIST_STD, UNDIST_LUT
		
		//tracker.setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_RGB565);
		//tracker.setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_RGB);
		tracker.setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);
		trackerTemplate.setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);
	}	
	
	env->ReleaseStringUTFChars(sdCardDir, nativeString);
	initialized++;
}

JNIEXPORT jlong JNICALL Java_com_artollkitplus_benchmark_MarkerInfo_artoolkitPlus_1detectmarkers(
		JNIEnv *env, jobject object, jbyteArray image, jobject transMatMonitor) {
	////////	
	clock_gettime(CLOCK_MONOTONIC, &startMarkerDetect);	
	////////
	jbyte* buff;
	buff = env->GetByteArrayElements(image, JNI_FALSE);
	uint8_t *cameraBuffer = (uint8_t*) buff;
	int best;
	float conf;
	vector<int> markerId;
	//find the camera pose
	if(tracker_mode == TRACKER_MULTI){
		if(marker_mode == TEMPLATE){
			markersCount = trackerMultiTemplate.calc(cameraBuffer);
		}else{
			markersCount = trackerMulti.calc(cameraBuffer);
		}
	}else if(tracker_mode == TRACKER_SINGLE){
		if(marker_mode == TEMPLATE){
			markerId = trackerTemplate.calc(cameraBuffer);
			best = trackerTemplate.selectBestMarkerByCf();
			conf = trackerTemplate.getConfidence();
		}else{
			markerId = tracker.calc(cameraBuffer);
			best = tracker.selectBestMarkerByCf();
			conf = tracker.getConfidence();
		}
		markersCount = markerId.size();
	}	
	cameraBuffer = NULL;
	env->ReleaseByteArrayElements(image, buff, 0);
	/////////////
	clock_gettime(CLOCK_MONOTONIC, &endMarkerDetect);
	timeElapsedMarkerDetection = timespecDiff(&endMarkerDetect, &startMarkerDetect);
	/////////////
	return timeElapsedMarkerDetection/1000000;
}

JNIEXPORT void JNICALL Java_com_artollkitplus_benchmark_MarkerInfo_artoolkitPlusDraw(JNIEnv *env,
		jobject object) {
	//setup the 3D environment
	GLfloat    gl_para[16];
	
	if(markersCount>0){
		if(tracker_mode == TRACKER_MULTI){
			if(marker_mode == TEMPLATE){
				for(int i = 0; i < trackerMultiTemplate.getMultiMarkerConfig()->marker_num; i++ ) {
					if( trackerMultiTemplate.getMultiMarkerConfig()->marker[i].visible >= 0 ){
						//////////////////////MULTI//////////////////////////////////
						glDisableClientState(GL_TEXTURE_COORD_ARRAY);
						glDisable(GL_TEXTURE_2D);
						glMatrixMode(GL_MODELVIEW);
						glLoadIdentity();
						glMatrixMode(GL_PROJECTION);
						//////////////////////////////
						glLoadMatrixf(trackerMultiTemplate.getProjectionMatrix() );

						glClearDepthf( 1.0 );
						glClear(GL_DEPTH_BUFFER_BIT);
						glEnable(GL_DEPTH_TEST);
						glDepthFunc(GL_LEQUAL);
						glMatrixMode(GL_MODELVIEW);
						glLoadMatrixf(trackerMultiTemplate.getModelViewMatrix());
						glScalef(.18f, .18f, .18f);
						////////////////////////////////////
						glEnable(GL_LIGHTING);
						glEnable(GL_LIGHT0);
						glLightfv(GL_LIGHT0, GL_POSITION, light_position);
						glLightfv(GL_LIGHT0, GL_AMBIENT, ambi);
						glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor);
						glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash);
						glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny);
						glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);

						//draw cube
						glColor4f(0, 1.0f, 0, 1.0f);
						//glTranslatef( 0.0, 0.0, -12.5 );

						glEnableClientState(GL_VERTEX_ARRAY);
						glEnableClientState(GL_NORMAL_ARRAY);

						glVertexPointer(3, GL_FLOAT, 0, box);
						glNormalPointer(GL_FLOAT,0, normals);
						glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 4, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 8, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 12, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 16, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 20, 4);
						glDisableClientState(GL_VERTEX_ARRAY);
						glDisableClientState(GL_NORMAL_ARRAY);
					}
				}
			}else{
				for(int i = 0; i < trackerMulti.getMultiMarkerConfig()->marker_num; i++ ) {
					if( trackerMulti.getMultiMarkerConfig()->marker[i].visible >= 0 ){
						glDisableClientState(GL_TEXTURE_COORD_ARRAY);
						glDisable(GL_TEXTURE_2D);
						glMatrixMode(GL_MODELVIEW);
						glLoadIdentity();
						glMatrixMode(GL_PROJECTION);
						///////////////
						glLoadMatrixf(trackerMulti.getProjectionMatrix() );
						glClearDepthf( 1.0 );
						glClear(GL_DEPTH_BUFFER_BIT);
						glEnable(GL_DEPTH_TEST);
						glDepthFunc(GL_LEQUAL);
						glMatrixMode(GL_MODELVIEW);
						glLoadMatrixf(trackerMulti.getModelViewMatrix() );
						glScalef(.18f, .18f, .18f);
						////////////////////////////////////
						glEnable(GL_LIGHTING);
						glEnable(GL_LIGHT0);
						glLightfv(GL_LIGHT0, GL_POSITION, light_position);
						glLightfv(GL_LIGHT0, GL_AMBIENT, ambi);
						glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor);
						glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash);
						glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny);
						glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);

						//draw cube
						glColor4f(0, 1.0f, 0, 1.0f);

						glEnableClientState(GL_VERTEX_ARRAY);
						glEnableClientState(GL_NORMAL_ARRAY);

						glVertexPointer(3, GL_FLOAT, 0, box);
						glNormalPointer(GL_FLOAT,0, normals);
						glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 4, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 8, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 12, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 16, 4);
						glDrawArrays(GL_TRIANGLE_STRIP, 20, 4);
						glDisableClientState(GL_VERTEX_ARRAY);
						glDisableClientState(GL_NORMAL_ARRAY);
					}
				}
			}
		}else if(tracker_mode == TRACKER_SINGLE){
			glDisableClientState(GL_TEXTURE_COORD_ARRAY);
			glDisable(GL_TEXTURE_2D);

			glMatrixMode(GL_MODELVIEW);
			glLoadIdentity();
			glMatrixMode(GL_PROJECTION);
			
			#ifdef USE_TEMPLATE_MARKERS	
			if(marker_mode == TEMPLATE){
				glLoadMatrixf( trackerTemplate.getProjectionMatrix() );
				glClearDepthf( 1.0 );
				glClear(GL_DEPTH_BUFFER_BIT);
				glEnable(GL_DEPTH_TEST);
				glDepthFunc(GL_LEQUAL);
				glMatrixMode(GL_MODELVIEW);
				glLoadMatrixf( trackerTemplate.getModelViewMatrix() );
				glScalef(.008f, .008f, .008f);
			}else{
				#endif
				glLoadMatrixf( tracker.getProjectionMatrix() );
				glClearDepthf( 1.0 );
				glClear(GL_DEPTH_BUFFER_BIT);
				glEnable(GL_DEPTH_TEST);
				glDepthFunc(GL_LEQUAL);
				glMatrixMode(GL_MODELVIEW);
				glLoadMatrixf( tracker.getModelViewMatrix() );
				glScalef(.01f, .01f, .01f);
				#ifdef USE_TEMPLATE_MARKERS	
			}
			#endif
			
			

			glEnable(GL_LIGHTING);
			glEnable(GL_LIGHT0);
			glLightfv(GL_LIGHT0, GL_POSITION, light_position);
			glLightfv(GL_LIGHT0, GL_AMBIENT, ambi);
			glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor);
			glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash);
			glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny);
			glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);

			//draw cube
			glColor4f(0, 1.0f, 0, 1.0f);
			glTranslatef( 0.0, 0.0, -12.5 );

			glEnableClientState(GL_VERTEX_ARRAY);
			glEnableClientState(GL_NORMAL_ARRAY);

			glVertexPointer(3, GL_FLOAT, 0, box);
			glNormalPointer(GL_FLOAT,0, normals);
			glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
			glDrawArrays(GL_TRIANGLE_STRIP, 4, 4);
			glDrawArrays(GL_TRIANGLE_STRIP, 8, 4);
			glDrawArrays(GL_TRIANGLE_STRIP, 12, 4);
			glDrawArrays(GL_TRIANGLE_STRIP, 16, 4);
			glDrawArrays(GL_TRIANGLE_STRIP, 20, 4);
			glDisableClientState(GL_VERTEX_ARRAY);
			glDisableClientState(GL_NORMAL_ARRAY);
		}	
	}
}

//to do get rid off gray8toRGB32
JNIEXPORT jint JNICALL Java_com_artollkitplus_benchmark_MarkerInfo_grayToRgb( JNIEnv*  env,
                                      jobject  object,
                                      jbyteArray src,
                                      jintArray  dst )
{
	jboolean b;
	jint len;
	jbyte* srcPtr=env->GetByteArrayElements(src,&b);
	jint* dstPtr=env->GetIntArrayElements(dst,&b);
	len = env->GetArrayLength(src);
	gray8toRGB32(srcPtr, dstPtr, len);
	env->ReleaseByteArrayElements( src, srcPtr, 0);
	env->ReleaseIntArrayElements( dst, dstPtr, 0);
}

void gray8toRGB32(jbyte* src, jint* dst, int len)
{
	int i,Y;
	for(i=0; i<len; i++){
		Y = *src & 0xff; 
		*dst = 0xff000000 + (Y << 16) + (Y << 8) + Y;
		src++;
		dst++;
	}
}

static void argConvGlpara(const ARFloat para[3][4], GLfloat gl_para[16] )
{
	int     i, j;
	for( j = 0; j < 3; j++ ) {
		for( i = 0; i < 4; i++ ) {
			gl_para[i*4+j] = para[j][i];
		}
	}
	gl_para[0*4+3] = gl_para[1*4+3] = gl_para[2*4+3] = 0.0;
	gl_para[3*4+3] = 1.0;
}

int64_t timespecDiff(struct timespec *timeA_p, struct timespec *timeB_p)
{
  return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) -
           ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec);
}


